#ifndef ROBORTS_DECISION_PREPARE_BEHAVIOR_H
#define ROBORTS_DECISION_PREPARE_BEHAVIOR_H
#define PI 3.1415926535
#include <ros/ros.h>
#include <geometry_msgs/Twist.h>

#include "io/io.h"
#include "math.h"
#include "../blackboard/blackboard.h"
#include "../executor/chassis_executor.h"
#include "../behavior_tree/behavior_state.h"
#include "../proto/decision.pb.h"
#include "../executor/gimbal_executor.h"

#include "line_iterator.h"
#include <unistd.h>
#include "std_msgs/Int16.h"
#include "nav_msgs/Odometry.h"
//扭腰动作，Movetype=3，还在完善中
namespace roborts_decision {
    class PrepareBehavior{
        public:
  PrepareBehavior(ChassisExecutor* &chassis_executor,
                GimbalExecutor* &gimbal_executor,
                Blackboard* &blackboard,
                const std::string & proto_file_path) : chassis_executor_(chassis_executor), 
                gimbal_executor_(gimbal_executor),
						blackboard_(blackboard){
                        max_angle = 0.4;
                        orien = -1;
			            p=0.9;
                        seq = 0;
                        rotating = false;
            }


    // void Reset()
    // {
    //     origin_gimbal = blackboard_->GetGimbalAngle();
    //     std::cout<<"reset"<<std::endl;
    // }


    void Run()
    {
        auto executor_state_chassis = Update_chassis();
        auto executor_state_gimbal = Update_gimbal();
        double cur_angle = blackboard_->GetGimbalEncoder();   //get angle between gimbal and base_link
        roborts_msgs::visual_detection target = blackboard_->GetTarget();
        ros::Time cur = ros::Time::now();
        double currentYaw = blackboard_->getCurrentIMU(cur);
        double pred_yaw = target.G_angle.yaw_angle - currentYaw;
        pred_yaw -= int(pred_yaw/2*PI)*2*PI;
        std::cout<<pred_yaw<<std::endl;
        if(cur_angle>0.9)   //pi/4
        {
            rotating = true;
            orien = 1;
        }
        else if(cur_angle<0.7)
        {
          orien = -1;
          rotating = true;
        }

        if(rotating)
        {
            if(cur_angle<0.8&&orien==1)
            {
                rotating = false;
                orien = 0;
            }
            if(cur_angle>0.8&&orien==-1)
            {
                rotating = false;
                orien = 0;
            }
        }


        // else if(cur_angle<-0.6)
        // {
        //     orien = -1;
        // }
        roborts_msgs::TwistAccel chassis_spd;
        chassis_spd.twist.angular.z = 1.5*orien;
        roborts_msgs::GimbalAngle ideal_angle;
        ideal_angle.yaw_mode = true;
        ideal_angle.pitch_mode = false;
        if(target.multicar[0].pose.header.seq  <= seq)
        {
            double rotate_angle = std::min(0.01*20,abs(rest_angle));
            ideal_angle.yaw_angle = rest_angle/abs(rest_angle) * rotate_angle;
            ideal_angle.yaw_angle = rest_angle;
            rest_angle -= ideal_angle.yaw_angle;
            ideal_angle.pitch_angle = target.G_angle.pitch_angle;
            //ideal_angle.yaw_angle -= blackboard_->GetOdom().twist.twist.angular.z*0.01;
            // ideal_angle.yaw_angle -= chassis_spd.twist.angular.z*0.008;
            ideal_angle.yaw_angle -= chassis_spd.twist.angular.z*0.010;
        }
        else
        {
            double rotate_angle = std::min(0.01*20,abs(pred_yaw));

            ideal_angle.yaw_angle = pred_yaw/abs(pred_yaw) * rotate_angle;
            ideal_angle.yaw_angle -= chassis_spd.twist.angular.z*0.010;
            // ideal_angle.yaw_angle -= chassis_spd.twist.angular.z *0.008;
            rest_angle =  pred_yaw- ideal_angle.yaw_angle;
            ideal_angle.pitch_angle = target.G_angle.pitch_angle;
        }

        gimbal_executor_->Execute(ideal_angle);
        chassis_executor_->Execute(chassis_spd);
        seq = target.multicar[0].pose.header.seq;
}

    void Cancel()
    {
        chassis_executor_->Cancel();
        gimbal_executor_->Cancel();
    }

    BehaviorState Update_chassis() {
    return chassis_executor_->Update();
      }

    BehaviorState Update_gimbal() {
    return gimbal_executor_->Update();
    }

     private:
        roborts_msgs::GimbalAngle gimbal_angle_execute;//最终发送的云台角度数据
         int seq;//扭腰计数器，以200为周期
         int orien;//记录方向，其值为+1或者-1
	    int max_angle;
	    double p;//回拉比例系数，默认0.01
	    double rest_angle;//初始位置角度
        bool rotating;
         roborts_msgs::TwistAccel t;
        ChassisExecutor* const chassis_executor_;
        GimbalExecutor *const gimbal_executor_; 
        Blackboard* const blackboard_;
        geometry_msgs::PoseStamped boot_position_;
        ros::Time last_time;
        roborts_msgs::GimbalAngle lastgae;
    };
}

#endif
